
/**
  ******************************************************************************
  * Copyright 2021 The grapilot Authors. All Rights Reserved.
  * 
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
  * You may obtain a copy of the License at
  * 
  * http://www.apache.org/licenses/LICENSE-2.0
  * 
  * Unless required by applicable law or agreed to in writing, software
  * distributed under the License is distributed on an "AS IS" BASIS,
  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  * See the License for the specific language governing permissions and
  * limitations under the License.
  * 
  * @file       rc_channels.c
  * @author     baiyang
  * @date       2021-8-8
  ******************************************************************************
  */

/*----------------------------------include-----------------------------------*/
#include "rc_channel.h"
#include <common/gp_math/gp_mathlib.h>
/*-----------------------------------macro------------------------------------*/

/*----------------------------------typedef-----------------------------------*/

/*---------------------------------prototype----------------------------------*/

/*----------------------------------variable----------------------------------*/

/*-------------------------------------os-------------------------------------*/

/*----------------------------------function----------------------------------*/
/**
  * @brief       
  * @param[in]   pRcs  
  * @param[out]  
  * @retval      
  * @note        
  */
void RCs_init_aux_all(RCs_HandleTypeDef *pRcs)
{
    for (uint8_t i=0; i<MAX_RC_CHANNEL_NUM; i++) {
        RC_init_aux(&(pRcs->channel[i]));
    }

    RCs_reset_mode_switch(pRcs);
}

/**
  * @brief       
  * @param[in]   pRcs  
  * @param[out]  
  * @retval      
  * @note        
  */
void RCs_read_aux_all(RCs_HandleTypeDef *pRcs)
{
    if (!RCs_has_valid_input()) {
        // exit immediately when no RC input
        return;
    }

    bool need_log = false;

    for (uint8_t i=0; i<MAX_RC_CHANNEL_NUM; i++) {
        need_log |= RC_read_aux(&(pRcs->channel[i]));
    }

    if (need_log) {
        // guarantee that we log when a switch changes
        
    }
}

/**
  * @brief       
  * @param[in]   pRc  
  * @param[out]  
  * @retval      
  * @note        
  */
void RCs_reset_mode_switch(RCs_HandleTypeDef *pRcs)
{
    RC_HandleTypeDef *c = RCs_flight_mode_channel(pRcs);
    if (c == NULL) {
        return;
    }

    RC_reset_mode_switch(c);
}

/**
  * @brief       
  * @param[in]   pRcs  
  * @param[out]  
  * @retval      
  * @note        
  */
void RCs_read_mode_switch(RCs_HandleTypeDef *pRcs)
{
    if (!RCs_has_valid_input()) {
        // exit immediately when no RC input
        return;
    }
    RC_HandleTypeDef *c = RCs_flight_mode_channel(pRcs);
    if (c == NULL) {
        return;
    }
    
    RC_read_mode_switch(c);
}


RC_HandleTypeDef *RCs_flight_mode_channel(RCs_HandleTypeDef *pRcs)
{
    const uint8_t num = RCs_flight_mode_channel_number();
    if (num <= 0) {
        return NULL;
    }
    if (num >= MAX_RC_CHANNEL_NUM) {
        return NULL;
    }
    return &(pRcs->channel[num-1]);
}

/**
  * @brief       
  * @param[in]   pRcs  
  * @param[out]  
  * @retval      
  * @note        
  */
bool RCs_ignore_receiver(RCs_HandleTypeDef *pRcs)
{
    return pRcs->_options & (uint32_t)RC_IGNORE_RECEIVER;
}

/**
  * @brief       如果我们有直接连接的RC接收器，则返回true，不包括覆盖
  * @param[in]   pRcs  
  * @param[out]  
  * @retval      
  * @note        
  */
bool RCs_has_had_rc_receiver(RCs_HandleTypeDef *pRcs)
{
    return pRcs->has_had_rc_receiver;
}

// returns true if overrides should time out.  If true is returned
// then returned_timeout_ms will contain the timeout in
// milliseconds, with 0 meaning overrides are disabled.
bool RCs_get_override_timeout_ms(RCs_HandleTypeDef *pRcs, uint32_t *returned_timeout_ms)
{
    const float val = pRcs->_override_timeout;
    if (math_flt_positive(val)) {
        *returned_timeout_ms = (uint32_t)(val * 1e3f);
        return true;
    }
    if (math_flt_zero(val)) {
        *returned_timeout_ms = 0;
        return true;
    }
    // overrides will not time out
    return false;
}

/**
  * @brief       
  * @param[in]   chans  
  * @param[in]   num_channels  
  * @param[out]  
  * @retval      
  * @note        
  */
uint8_t RCs_get_radio_in(RCs_HandleTypeDef *pRcs, uint16_t *chans, const uint8_t num_channels)
{
    rt_memset(chans, 0, num_channels*sizeof(*chans));

    const uint8_t read_channels = MIN(num_channels, MAX_RC_CHANNEL_NUM);
    for (uint8_t i = 0; i < read_channels; i++) {
        chans[i] = pRcs->channel[i].radio_in;
    }

    return read_channels;
}

/**
  * @brief       
  * @param[in]   
  * @param[out]  
  * @retval      
  * @note        
  */
void RCs_clear_overrides(RCs_HandleTypeDef *pRcs)
{
     for (uint8_t i = 0; i < MAX_RC_CHANNEL_NUM; i++) {
         RC_clear_override(&(pRcs->channel[i]));
     }
     // we really should set has_new_overrides to true, and rerun read_input from
     // the vehicle code however doing so currently breaks the failsafe system on
     // copter and plane, RC_Channels needs to control failsafes to resolve this
}

// should a channel reverse option affect aux switches
bool RCs_switch_reverse_allowed(RCs_HandleTypeDef *pRcs)
{
    return pRcs->_options & (uint32_t)RC_ALLOW_SWITCH_REV;
}

/**
  * @brief       
  * @param[in]   pRcs  
  * @param[out]  
  * @retval      
  * @note        
  */
bool RCs_has_active_overrides(RCs_HandleTypeDef *pRcs)
{
    for (uint8_t i = 0; i < MAX_RC_CHANNEL_NUM; i++) {
        if (RC_has_override(&(pRcs->channel[i]))) {
            return true;
        }
    }

    return false;
}

/**
  * @brief       
  * @param[in]   pRcs  
  * @param[in]   option  
  * @param[out]  
  * @retval      
  * @note        
  */
RC_HandleTypeDef *RCs_find_channel_for_option(RCs_HandleTypeDef *pRcs, const RC_AuxFunc_t option)
{
    for (uint8_t i=0; i<MAX_RC_CHANNEL_NUM; i++) {
        RC_HandleTypeDef *c = &(pRcs->channel[i]);
        if (c == NULL) {
            // odd?
            continue;
        }
        if ((RC_AuxFunc_t)c->_option == option) {
            return c;
        }
    }
    return NULL;
}

// duplicate_options_exist - returns true if any options are duplicated
bool RCs_duplicate_options_exist(RCs_HandleTypeDef *pRcs)
{
    uint8_t auxsw_option_counts[256] = {0};
    for (uint8_t i=0; i<MAX_RC_CHANNEL_NUM; i++) {
        const RC_HandleTypeDef *c = &(pRcs->channel[i]);
        if (c == NULL) {
            // odd?
            continue;
        }
        const uint16_t option = c->_option;
        if (option >= sizeof(auxsw_option_counts)) {
            continue;
        }
        auxsw_option_counts[option]++;
    }

    for (uint16_t i=0; i<sizeof(auxsw_option_counts); i++) {
        if (i == 0) { // MAGIC VALUE! This is AUXSW_DO_NOTHING
            continue;
        }
        if (auxsw_option_counts[i] > 1) {
            return true;
        }
    }
   return false;
}

// check if flight mode channel is assigned RC option
// return true if assigned
bool RCs_flight_mode_channel_conflicts_with_rc_option(RCs_HandleTypeDef *pRcs)
{
    RC_HandleTypeDef *chan = RCs_flight_mode_channel(pRcs);
    if (chan == NULL) {
        return false;
    }
    return (RC_AuxFunc_t)chan->_option != RC_DO_NOTHING;
}

/*------------------------------------test------------------------------------*/


